#include <cstdio>
#include <iostream>

#include "ros/ros.h"
// #include "utility.hpp"
#include <camera_info_manager/camera_info_manager.h>

#include "sensor_msgs/Image.h"

// Inludes common necessary includes for development using depthai library
#include <vision_msgs/Detection2DArray.h>

#include <depthai_bridge/BridgePublisher.hpp>
#include <depthai_bridge/ImageConverter.hpp>
#include <depthai_bridge/ImgDetectionConverter.hpp>
#include <depthai_bridge/ImuConverter.hpp>

#include "depthai/depthai.hpp"
int main(int argc, char** argv) {
    ros::init(argc, argv, "oak_ffc");
    ros::NodeHandle pnh("~");

    std::string tfPrefix;
    std::string camera_param_uri;
    int badParams = 0;
    int exposure_time;
    int sensitivity_iso;
    std::string yoloBlobPath;
    bool manual_exposure = false;
    badParams += !pnh.getParam("tf_prefix", tfPrefix);
    badParams += !pnh.getParam("camera_param_uri", camera_param_uri);
    badParams += !pnh.getParam("exposure_time", exposure_time);
    badParams += !pnh.getParam("sensitivity_iso", sensitivity_iso);
    badParams += !pnh.getParam("yolo_blob", yoloBlobPath);
    badParams += !pnh.getParam("manual_exposure", manual_exposure);
    if(badParams > 0) {
        throw std::runtime_error("Couldn't find one of the parameters");
    }

    dai::Pipeline pipeline;

    auto rgbCam = pipeline.create<dai::node::ColorCamera>();
    auto cam0 = pipeline.create<dai::node::MonoCamera>();
    auto cam1 = pipeline.create<dai::node::MonoCamera>();

    auto imu = pipeline.create<dai::node::IMU>();
    imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
    imu->setBatchReportThreshold(1);
    imu->setMaxBatchReports(2);

    rgbCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P);
    rgbCam->setPreviewSize(416, 416);
    rgbCam->setInterleaved(false);
    rgbCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR);
    rgbCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
    rgbCam->setFps(20);
    cam0->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    cam0->setBoardSocket(dai::CameraBoardSocket::CAM_B);

    cam1->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
    cam1->setBoardSocket(dai::CameraBoardSocket::CAM_C);

    auto rgbxlinkOut = pipeline.create<dai::node::XLinkOut>();
    auto cam0LinkOut = pipeline.create<dai::node::XLinkOut>();
    auto cam1LinkOut = pipeline.create<dai::node::XLinkOut>();

    auto xoutImu = pipeline.create<dai::node::XLinkOut>();
    auto xoutNN = pipeline.create<dai::node::XLinkOut>();

    rgbxlinkOut->setStreamName("rgb");
    cam0LinkOut->setStreamName("left");
    cam1LinkOut->setStreamName("right");
    xoutImu->setStreamName("imu");
    xoutNN->setStreamName("detections");

    rgbCam->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::OUTPUT);
    cam0->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::OUTPUT);
    cam1->initialControl.setFrameSyncMode(dai::CameraControl::FrameSyncMode::INPUT);
    if(manual_exposure) {
        // rgbCam->initialControl.setManualExposure(exposure_time, sensitivity_iso);
        cam0->initialControl.setManualExposure(exposure_time, sensitivity_iso);
        cam1->initialControl.setManualExposure(exposure_time, sensitivity_iso);
    }
    // // Link plugins CAM -> XLINK
    cam0->out.link(cam0LinkOut->input);
    cam1->out.link(cam1LinkOut->input);

    imu->out.link(xoutImu->input);

    auto detectionNetwork = pipeline.create<dai::node::YoloDetectionNetwork>();
    // Network specific settings
    detectionNetwork->setConfidenceThreshold(0.5f);
    detectionNetwork->setNumClasses(80);
    detectionNetwork->setCoordinateSize(4);
    detectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319});
    detectionNetwork->setAnchorMasks({{"side26", {1, 2, 3}}, {"side13", {3, 4, 5}}});
    detectionNetwork->setIouThreshold(0.5f);
    detectionNetwork->setBlobPath(yoloBlobPath);
    detectionNetwork->setNumInferenceThreads(2);
    detectionNetwork->input.setBlocking(false);
    rgbCam->preview.link(detectionNetwork->input);
    // detectionNetwork->passthrough.link(rgbxlinkOut->input);
    rgbCam->preview.link(rgbxlinkOut->input);
    detectionNetwork->out.link(xoutNN->input);

    dai::Device device(pipeline);
    dai::Device::Config c = pipeline.getDeviceConfig();
    c.board.gpio[6] = dai::BoardConfig::GPIO(dai::BoardConfig::GPIO::Direction::OUTPUT,
                                             dai::BoardConfig::GPIO::Level::HIGH);
    std::shared_ptr<dai::DataOutputQueue> rgbQueue = device.getOutputQueue("rgb", 1, false);
    std::shared_ptr<dai::DataOutputQueue> leftQueue = device.getOutputQueue("left", 1, false);
    std::shared_ptr<dai::DataOutputQueue> rightQueue = device.getOutputQueue("right", 1, false);
    std::shared_ptr<dai::DataOutputQueue> imuQueue = device.getOutputQueue("imu", 10, false);

    std::shared_ptr<dai::DataOutputQueue> detectionQueue =
        device.getOutputQueue("detections", 1, false);

    dai::rosBridge::ImuConverter imuConverter(
        tfPrefix + "_imu_frame", dai::ros::ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL, 0.0, 0.0);
    dai::rosBridge::BridgePublisher<sensor_msgs::Imu, dai::IMUData> imuPublish(
        imuQueue,
        pnh,
        std::string("imu"),
        std::bind(&dai::rosBridge::ImuConverter::toRosMsg,
                  &imuConverter,
                  std::placeholders::_1,
                  std::placeholders::_2),
        1,
        "",
        "imu");
    imuPublish.addPublisherCallback();

    std::string color_uri = camera_param_uri + "/" + "color.yaml";
    dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
    dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> rgbPublish(
        rgbQueue,
        pnh,
        std::string("front/image"),
        std::bind(&dai::rosBridge::ImageConverter::toRosMsg,
                  &rgbConverter,  // since the converter has the same frame name
                                  // and image type is also same we can reuse it
                  std::placeholders::_1,
                  std::placeholders::_2),
        1,
        color_uri,
        "front");

    rgbPublish.addPublisherCallback();

    dai::rosBridge::ImageConverter cam0Converter(tfPrefix + "_cam0_camera_optical_frame", false);
    dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> cam0Publish(
        leftQueue,
        pnh,
        std::string("back2/image"),
        std::bind(&dai::rosBridge::ImageConverter::toRosMsg,
                  &cam0Converter,  // since the converter has the same frame name
                                   // and image type is also same we can reuse it
                  std::placeholders::_1,
                  std::placeholders::_2),
        1,
        color_uri,
        "right");

    cam0Publish.addPublisherCallback();

    dai::rosBridge::ImageConverter cam1Converter(tfPrefix + "_cam1_camera_optical_frame", false);
    dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> cam1Publish(
        rightQueue,
        pnh,
        std::string("back1/image"),
        std::bind(&dai::rosBridge::ImageConverter::toRosMsg,
                  &cam1Converter,  // since the converter has the same frame name
                                   // and image type is also same we can reuse it
                  std::placeholders::_1,
                  std::placeholders::_2),
        1,
        color_uri,
        "back");
    cam1Publish.addPublisherCallback();

    dai::rosBridge::ImgDetectionConverter detConverter(
        tfPrefix + "_rgb_camera_optical_frame", 416, 416, false);
    dai::rosBridge::BridgePublisher<vision_msgs::Detection2DArray, dai::ImgDetections>
        detectionPublish(detectionQueue,
                         pnh,
                         std::string("yolov4_detections"),
                         std::bind(&dai::rosBridge::ImgDetectionConverter::toRosMsg,
                                   &detConverter,
                                   std::placeholders::_1,
                                   std::placeholders::_2),
                         1);

    detectionPublish.addPublisherCallback();
    ros::spin();
    return 0;
}
